#include "cube.h"
#include <vector>

const Point Cube::points[8] = {	Point(-0.5,-0.5,0.5),Point(0.5,-0.5,0.5),Point(0.5,0.5,0.5),Point(-0.5,0.5,0.5),
								Point(-0.5,-0.5,-0.5),Point(0.5,-0.5,-0.5),Point(0.5,0.5,-0.5),Point(-0.5,0.5,-0.5)	};

Cube::Cube(){
}

Cube::Cube(const Point& p1,const Point p2){
	createCubePoints(p1,p2);
}


Cube::Cube(float x, float y, float z){
	createCubePoints(Point(x,y,z),Point(x,y,z));
}

Cube& Cube::translate(float dx,float dy,float dz){
	Matrix4x4 t1(dx,dy,dz);

	for(unsigned int c=0;c<4;c++){
		_frontFace[c]=t1*_frontFace[c];
		_backFace[c]=t1*_backFace[c];
	}

	return *this;
}

Point Cube::center()const{
	return ((_frontFace[0]-_backFace[2])*0.5) + _backFace[2];
}

float Cube::width()const{
	return (_frontFace[0]-_frontFace[1]).mod();
}

float Cube::height()const{
	return (_frontFace[0]-_backFace[0]).mod();
}

float Cube::depth()const{
	return (_frontFace[0]-_frontFace[3]).mod();
}


Cube& Cube::center(const Point& center){
	Point p = center - this->center();
	this->translate(p.x1,p.x2,p.x3);

	return *this;
}

const Point* Cube::getNormalizedPoints(){
	return points;
}

void Cube::rotate(float angle,const Point& center,const Point& axis){
	Matrix4x4 t1(-center.x1,-center.x2,-center.x3);
	Matrix4x4 r1(angle,axis);
	Matrix4x4 t2(center.x1,center.x2,center.x3);
	Matrix4x4 m;
	m*=t1;
	m*=r1;
	m*=t2;

	for(unsigned int c=0;c<4;c++){
		_frontFace[c]=m*_frontFace[c];
		_backFace[c]=m*_backFace[c];
	}
}

bool Cube::intersect(const Cube& c2){
	return intersect(c2,false);
}

float Cube::minZ()const{
	return min(3);
}

float Cube::maxZ()const{
	return max(3);
}

float Cube::minX()const{
	return min(1);
}

float Cube::maxX()const{
	return max(1);
}

float Cube::minY()const{
	return min(2);
}

float Cube::maxY()const{
	return max(2);
}

float Cube::max(unsigned int coord)const{
	float _max = _frontFace[0][coord];

	for(unsigned int c=0;c<4;c++){
		if(_frontFace[c][coord]>_max)
			_max = _frontFace[c][coord];
		if(_backFace[c][coord]>_max)
			_max = _backFace[c][coord];
	}

	return _max;
}

float Cube::min(unsigned int coord)const{
	float _min = _frontFace[0][coord];

	for(unsigned int c=0;c<4;c++){
		if(_frontFace[c][coord]<_min)
			_min = _frontFace[c][coord];
		if(_backFace[c][coord]<_min)
			_min = _backFace[c][coord];
	}

	return _min;
}

bool Cube::operator <(const Cube& cube)const{
	return(this->minZ()<cube.minZ());
}

Cube& Cube::extend(float dx,float dy,float dz){
	std::set<unsigned int> indices;
	Point normal[6];
	Point dir(dx,dy,dz);
	calculateNormals(normal);

	for(unsigned int j=0;j<6;j++){
		if(dir*normal[j]>0){
			getFaceIndices(indices,j);
		}
	}


	for(std::set<unsigned int>::iterator it=indices.begin();it!=indices.end();++it){
		unsigned int val = *it;
		Matrix4x4 t1(dx,dy,dz);
		if(val<4)
			_frontFace[val]=t1*_frontFace[val];
		else
			_backFace[val-4]=t1*_backFace[val-4];
	}

	return *this;
}

Cube& Cube::shrink(float dx,float dy,float dz){
	std::set<unsigned int> indices;
	Point normal[6];
	Point dir(-dx,-dy,-dz);
	calculateNormals(normal);

	for(unsigned int j=0;j<6;j++){
		if(dir*normal[j]<0){
			getFaceIndices(indices,j);
		}
	}


	for(std::set<unsigned int>::iterator it=indices.begin();it!=indices.end();++it){
		unsigned int val = *it;
		Matrix4x4 t1(-dx,-dy,-dz);
		if(val<4)
			_frontFace[val]=t1*_frontFace[val];
		else
			_backFace[val-4]=t1*_backFace[val-4];
	}

	return *this;
}

void Cube::getFaceIndices(std::set<unsigned int>& indices,unsigned int faceNumber){
	switch(faceNumber){
		case 0:
			indices.insert(4);
			indices.insert(5);
			indices.insert(6);
			indices.insert(7);
			break;
		case 1:
			indices.insert(0);
			indices.insert(1);
			indices.insert(2);
			indices.insert(3);
			break;
		case 2:
			indices.insert(1);
			indices.insert(2);
			indices.insert(5);
			indices.insert(6);
			break;
		case 3:
			indices.insert(0);
			indices.insert(3);
			indices.insert(4);
			indices.insert(7);
			break;
		case 4:
			indices.insert(2);
			indices.insert(3);
			indices.insert(6);
			indices.insert(7);
			break;
		case 5:
			indices.insert(0);
			indices.insert(1);
			indices.insert(4);
			indices.insert(5);
			break;
	}
}

void Cube::calculateNormals(Point* normal)const{
	Point nullPoint;

	normal[0]  = _backFace[0]-_frontFace[0];
	normal[2]  = _frontFace[1]-_frontFace[0];
	normal[4]  = _frontFace[2]-_frontFace[1];

	if(normal[0]==nullPoint){
		normal[0] = normal[2].cross(normal[4]);
	}
	else if(normal[2]==nullPoint){
		normal[2] = normal[0].cross(normal[4]);
	}
	else if(normal[4]==nullPoint){
		normal[4] = normal[0].cross(normal[2]);
	}

	normal[1]  = normal[0]*-1;
	normal[3]  = normal[2]*-1;
	normal[5]  = normal[4]*-1;
}

bool Cube::intersect(const Cube& c2,bool recursive)const{
	unsigned int pointsIndex[6] = { 3,1,1,3,3,1 };
	Point normal[6];
	Point nullPoint;
	calculateNormals(normal);

	std::vector<unsigned int> faceCount(6);
	const Point* face;
	const Point* c2Face = c2._frontFace;
	for(unsigned int i=0,k=0;i<8;i++){
		for(unsigned int j=0;j<6;j++){
			if(!(j%2))
				face=_backFace;
			else
				face=_frontFace;

			Point p= c2Face[k]-face[pointsIndex[j]];
			float val = (p*normal[j]);
			if(val>0)
				faceCount[j]++;
		}

		k++;
		if(i==3){
			c2Face = c2._backFace;
			k=0;
		}
	}

	for(unsigned int c=0;c<6;c++){
		if(faceCount[c]==8)
			return false;
	}

	return true;
}

Cube& Cube::advance(const Point& direction,float amount){
	Point pm = direction*amount;
	this->translate(pm.x1,pm.x2,pm.x3);

	return *this;
}

Cube& Cube::advance(const Point& direction){
	Point pm = direction;
	this->translate(pm.x1,pm.x2,pm.x3);

	return *this;
}


void Cube::createCubePoints(const Point& p1,const Point p2){
	if(p1.x3>p2.x3){
		_frontFace[0]=Point(p1.x1,p1.x2,p1.x3);
		_frontFace[1]=Point(p2.x1,p1.x2,p1.x3);
		_frontFace[2]=Point(p2.x1,p2.x2,p1.x3);
		_frontFace[3]=Point(p1.x1,p2.x2,p1.x3);

		_backFace[0]=Point(p1.x1,p1.x2,p2.x3);
		_backFace[1]=Point(p2.x1,p1.x2,p2.x3);
		_backFace[2]=Point(p2.x1,p2.x2,p2.x3);
		_backFace[3]=Point(p1.x1,p2.x2,p2.x3);
	}else{
		_backFace[0]=Point(p1.x1,p1.x2,p1.x3);
		_backFace[1]=Point(p2.x1,p1.x2,p1.x3);
		_backFace[2]=Point(p2.x1,p2.x2,p1.x3);
		_backFace[3]=Point(p1.x1,p2.x2,p1.x3);

		_frontFace[0]=Point(p1.x1,p1.x2,p2.x3);
		_frontFace[1]=Point(p2.x1,p1.x2,p2.x3);
		_frontFace[2]=Point(p2.x1,p2.x2,p2.x3);
		_frontFace[3]=Point(p1.x1,p2.x2,p2.x3);
	}
}

